#include "vcu_driver_core.h"

Vcu::Vcu():nh_(""), private_nh_("~"){
    private_nh_.param<std::string>("input_movebase_cmd_topic_", input_movebase_cmd_topic_, "");
    private_nh_.param<std::string>("output_canframe_topic_", output_canframe_topic_, "");
    private_nh_.param<double>("track_base_", track_base_, 0.0);
    private_nh_.param<double>("track_radius_", track_radius_, 0.0);
}

Vcu::~Vcu(){}

void Vcu::run(){
    movebase_cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(input_movebase_cmd_topic_, 1, &Vcu::ctrl_cmd_callback, this);
    sent_canframe_pub_ = nh_.advertise<can_msgs::Frame>(output_canframe_topic_, 1);
    this->vehicle_initialized();
    ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &Vcu::process, this, false, true);
    ros::spin();
}

void Vcu::ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg){
    geometry_msgs::TwistStamped twist_;
    twist_.header.stamp = ros::Time().now();
    twist_.header.frame_id = "base_link";
    twist_.twist = *msg;
    movebase_ctrl_cmd_ = std::make_shared<geometry_msgs::TwistStamped>(twist_);
}

void Vcu::vehicle_initialized(){
    /* 设置左右电机进入闭环 */
    can_msgs::Frame sent_can_frame_;
    sent_can_frame_.header.stamp = ros::Time().now();
    sent_can_frame_.id = 0x047;
    sent_can_frame_.is_extended = false;
    sent_can_frame_.dlc = 0x08;
    sent_can_frame_.data[0] = 0x08;
    sent_can_frame_.data[1] = 0x00;
    sent_can_frame_.data[2] = 0x00;
    sent_can_frame_.data[3] = 0x00;
    sent_can_frame_.data[4] = 0x00;
    sent_can_frame_.data[5] = 0x00;
    sent_can_frame_.data[6] = 0x00;
    sent_can_frame_.data[7] = 0x00;
    sent_canframe_pub_.publish(sent_can_frame_);

    sent_can_frame_.header.stamp = ros::Time().now();
    sent_can_frame_.id = 0x007;
    sent_canframe_pub_.publish(sent_can_frame_);
}

void Vcu::process(const ros::TimerEvent& e){
    double vx, angular;
    if(movebase_ctrl_cmd_==nullptr || fabs(ros::Time().now().toSec()-movebase_ctrl_cmd_->header.stamp.toSec())>1.0){
        vx = 0.0;
        angular = 0.0;
    }else{
        vx = movebase_ctrl_cmd_->twist.linear.x;
        angular = movebase_ctrl_cmd_->twist.angular.z;
    }

    /* 计算左轮和右轮的线速度 */
    double vl, vr;
    vr = vx + angular*track_base_/2.0;
    vl = vx - angular*track_base_/2.0;

    ROS_INFO("[%s] vl=%lf, vr=%lf", __APP_NAME__, vl, vr);


    /* 计算左右轮转速 rps */
    float rps_l, rps_r;
    rps_l = vl/(M_PI*2*track_radius_);
    rps_r = vr/(M_PI*2*track_radius_);
    ROS_INFO("[%s] left_rps=%lf, right_rps=%lf", __APP_NAME__, rps_l, rps_r);

    unsigned char bytes[4];
    uint32_t uint_num = *(reinterpret_cast<uint32_t*>(&rps_l));  
    for (int i = 0; i < 4; i++) {  
        bytes[i] = uint_num & 0xFF;  
        uint_num >>= 8;  
    }  
  
    // 打印每个字节的值  
    // for (int i = 0; i < 4; i++) {  
    //     std::cout << std::bitset<8>(bytes[i]) << " "<<std::endl;
    //     ROS_INFO("[%s] left, %x", __APP_NAME__, bytes[i]);
    // }  

    /* 下发CAN指令 */
    can_msgs::Frame sent_can_frame_;
    sent_can_frame_.header.stamp = ros::Time().now();
    sent_can_frame_.id = 0x04d;
    sent_can_frame_.is_extended = false;
    sent_can_frame_.dlc = 0x08;
    sent_can_frame_.data[0] = bytes[0];
    sent_can_frame_.data[1] = bytes[1];
    sent_can_frame_.data[2] = bytes[2];
    sent_can_frame_.data[3] = bytes[3];
    sent_can_frame_.data[4] = 0x00;
    sent_can_frame_.data[5] = 0x00;
    sent_can_frame_.data[6] = 0x00;
    sent_can_frame_.data[7] = 0x00;
    sent_canframe_pub_.publish(sent_can_frame_);

    uint_num = *(reinterpret_cast<uint32_t*>(&rps_r));  
    for (int i = 0; i < 4; i++) {  
        bytes[i] = uint_num & 0xFF;  
        uint_num >>= 8;  
    }

    // for (int i = 0; i < 4; i++) {  
    //     std::cout << std::bitset<8>(bytes[i]) << " "<<std::endl;
    //     ROS_INFO("[%s] right, %x", __APP_NAME__, bytes[i]);
    // }

    sent_can_frame_.header.stamp = ros::Time().now();
    sent_can_frame_.id = 0x00d;
    sent_can_frame_.is_extended = false;
    sent_can_frame_.dlc = 0x08;
    sent_can_frame_.data[0] = bytes[0];
    sent_can_frame_.data[1] = bytes[1];
    sent_can_frame_.data[2] = bytes[2];
    sent_can_frame_.data[3] = bytes[3];
    sent_can_frame_.data[4] = 0x00;
    sent_can_frame_.data[5] = 0x00;
    sent_can_frame_.data[6] = 0x00;
    sent_can_frame_.data[7] = 0x00;
    sent_canframe_pub_.publish(sent_can_frame_);
}

